package com.xpstem.lelink;

import com.xpstem.lelink.command.EV3Command;
import com.xpstem.lelink.command.EV3DirectCommand;
import com.xpstem.lelink.device.EV3ServoMotor;
import com.xpstem.lelink.enums.Brake;
import com.xpstem.lelink.enums.OutputPort;
import com.xpstem.lelink.enums.Value;
import com.xpstem.lelink.operation.EV3Operation;
import com.xpstem.lelink.operation.EV3OutputOpFactory;
import com.xpstem.lelink.type.Data16;
import com.xpstem.lelink.type.Data32;
import com.xpstem.lelink.type.Data8;

import java.io.IOException;
import java.util.Optional;

/**
 * EV3 two wheel vehicle.
 *
 * @author billy zhang
 */
public class EV3TwoWheelVehicle {

    public static class Location {
        private float x;
        private float y;

        public Location() {
            this(0, 0);
        }

        public Location(float x, float y) {
            this.x = x;
            this.y = y;
        }

        public Location add(int distance, float rotateAndle) {
            return new Location(1, 1);
        }
    }

    private EventBus eventBus;
    private EV3ServoMotor leftMotor;
    private EV3ServoMotor rightMotor;
    /**
     * 车身位置，相对坐标0点
     */
    private Location location;
    /**
     * 旋转角度
     */
    private float rotateAngle;
    /**
     * 轮半径
     */
    private float wheelRadius;

    protected EV3TwoWheelVehicle(EventBus eventBus, EV3ServoMotor leftMotor, EV3ServoMotor rightMotor, float wheelRadius) {
        this.eventBus = eventBus;
        this.leftMotor = leftMotor;
        this.rightMotor = rightMotor;
        this.wheelRadius = wheelRadius;
    }

    public void stop(Brake brake) throws IOException {
        EV3Operation stopOp = EV3OutputOpFactory.opOutputStop(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                brake);
        EV3Command cmd = new EV3DirectCommand(stopOp);
        eventBus.postCommand(cmd);
    }

    /**
     * 直线移动
     *
     * @param speed
     * @param distince
     * @throws IOException
     */
    public void moveStraight(int speed, float distince) throws IOException {

        // 计算圈数

        // 移动

    }

    /**
     * 移动到指定位置
     *
     * @param speed
     * @param dist
     * @throws IOException
     */
    public void moveTo(int speed, Location dist) throws IOException {

        // 计算角度
        // 原地旋转

        // 计算圈数
        // 移动

        //location = dist;
    }

    /**
     * 左转指定度数
     *
     * @param speed
     * @param degree
     * @throws IOException
     */
    public void turnLeft(int speed, int degree) throws IOException {

        // 计算新位置
    }

    /**
     * 右转指定度数
     *
     * @param speed
     * @param degree
     * @throws IOException
     */
    public void turnRight(int speed, int degree) throws IOException {

        // 计算新位置
    }

    /**
     * 运行指定圈数。
     * 圈数 = step1 + step2 + step3
     *
     * @param power 指定功率
     * @param degree1 从0加速到指定功能率运行圈数，决定启动是否平稳
     * @param degree2 按指定功率运行的圈数
     * @param degree3 从指定功率减速到0所运行圈数，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void moveOnStepPower(int power, int degree1, int degree2, int degree3, Brake brake) throws IOException {
        int degree = leftMotor.getSensor().readyRaw(EV3ServoMotor.MODE_DEGREE);

        EV3Operation stepPowerOp = EV3OutputOpFactory.opOutputStepPower(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                Data8.from(Value.power(power)),
                Data32.from(degree1),
                Data32.from(degree2),
                Data32.from(degree3),
                brake);
        start(stepPowerOp);

        // 计算度数
        int newDegree = leftMotor.getSensor().readyRaw(EV3ServoMotor.MODE_DEGREE);
        updateLocation(newDegree - degree);
    }

    private void updateLocation(int degree) {
        int distance = (int)(Math.PI * wheelRadius * 2 * degree);
        location = location.add(distance, rotateAngle);
    }

    /**
     * 运行指定时长。
     * 时长 = time1 + time2 + time3
     *
     * @param power 指定功率
     * @param time1 从0加速到指定功能率运行时间，决定启动是否平稳
     * @param time2 按指定功率运行的时间
     * @param time3 从指定功率减速到0所运行时间，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void moveOnTimePower(int power, int time1, int time2, int time3, Brake brake) throws IOException {
        int degree = leftMotor.getSensor().readyRaw(EV3ServoMotor.MODE_DEGREE);

        EV3Operation timePowerOp = EV3OutputOpFactory.opOutputTimePower(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                Data8.from(Value.power(power)),
                Data32.from(time1),
                Data32.from(time2),
                Data32.from(time3),
                brake);
        start(timePowerOp);

        // 计算度数
        int newDegree = leftMotor.getSensor().readyRaw(EV3ServoMotor.MODE_DEGREE);
        updateLocation(newDegree - degree);
    }

    private void start(EV3Operation op) throws IOException {
        EV3Operation startOp = EV3OutputOpFactory.opOutputStart(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() } );
        EV3Command cmd = new EV3DirectCommand(op, startOp);
        eventBus.postCommand(cmd);
    }

    /**
     * 运行指定圈数。
     * 圈数 = step1 + step2 + step3
     *
     * @param speed 指定速度
     * @param degree1 从0加速到指定功能率运行圈数，决定启动是否平稳
     * @param degree2 按指定功率运行的圈数
     * @param degree3 从指定功率减速到0所运行圈数，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void moveOnStepSpeed(int speed, int degree1, int degree2, int degree3, Brake brake) throws IOException {

        EV3Operation stepSpeedOp = EV3OutputOpFactory.opOutputStepSpeed(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                Data8.from(Value.speed(speed)),
                Data32.from(degree1),
                Data32.from(degree2),
                Data32.from(degree3),
                brake);
        start(stepSpeedOp);

        // 计算新位置
    }

    /**
     * 运行指定时长。
     * 时长 = time1 + time2 + time3
     *
     * @param speed 指定功率
     * @param time1 从0加速到指定功能率运行时间，决定启动是否平稳
     * @param time2 按指定功率运行的时间
     * @param time3 从指定功率减速到0所运行时间，决定停止是否平稳
     * @param brake
     * @throws IOException
     */
    public void moveOnTimeSpeed(int speed, int time1, int time2, int time3, Brake brake) throws IOException {

        EV3Operation timeSpeedOp = EV3OutputOpFactory.opOutputTimeSpeed(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                Data8.from(Value.speed(speed)),
                Data32.from(time1),
                Data32.from(time2),
                Data32.from(time3),
                brake);
        start(timeSpeedOp);

        // 计算新位置
    }

    /**
     * 运行指定圈数。
     *
     * @param speed 指定速度
     * @param turn
     * @param degree
     * @param brake
     * @throws IOException
     */
    private void moveOnStepSync(int speed, int turn, int degree, Brake brake) throws IOException {

        EV3Operation stepSyncOp = EV3OutputOpFactory.opOutputStepSync(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                Data8.from(Value.speed(speed)),
                Data16.from(turn),
                Data32.from(degree),
                brake);
        start(stepSyncOp);

    }

    /**
     * 运行指定时长。
     *
     * @param speed 指定功率
     * @param turn
     * @param time
     * @param brake
     * @throws IOException
     */
    private void moveOnTimeSync(int speed, int turn, int time, Brake brake) throws IOException {
        EV3Operation timeSyncOp = EV3OutputOpFactory.opOutputTimeSync(
                eventBus.getLayer(),
                new OutputPort[] { leftMotor.getOutPort(), rightMotor.getOutPort() },
                Data8.from(Value.speed(speed)),
                Data16.from(turn),
                Data32.from(time),
                brake);
        start(timeSyncOp);
        
    }

}
